import HWT9073_CAN
import time
import os
import sys
import psutil
import csv
import numpy as np
import multiprocessing.shared_memory
import pwd
import grp
import threading
import logging
import datetime

Datafolder = 'Data/IMUData/'
formatted_time = time.strftime('%Y%m%d%H%M%S',time.localtime())
IMU_data_file = Datafolder + f'IMU_data_{formatted_time}.csv'


# 进程查询，防止多运行
def checkProcess(processName):
    # processName = os.path.basename(__file__)
    print("进程查询：{}".format(processName))
    ps1=psutil.pids()
    curps=os.getpid()
    for pid in ps1:
        try:
            cmdline = psutil.Process(pid).cmdline()
        except psutil.NoSuchProcess:
            continue
        # print(cmdline)
        if processName in cmdline and pid != curps and pid != (curps - 1):
            print(f"{processName}:, This is the second process pid = {str(pid)}, so exit!")
            sys.exit(1)
    print(f"进程查询完毕, 无抢占运行情况, now runing {processName},current pid = "+str(curps))
    return

def checkLastCallTime():
    global last_call_time
    time.sleep(30)  # 先等待30秒，使其数据稳定
    while True:
        if time.time() - last_call_time > 10:  # Adjust the time threshold as needed
            subProcStatus[0] = 0x09
        time.sleep(1)  # Adjust the sleep interval as needed

def updateData(DeviceModel, timestamp):
    """
    print(
        "ID:{}  {}{}  AccX:{}  AccY:{}  AccZ:{}  AsX:{}  AsY:{}  AsZ:{}  AngX:{}  AngY:{}  AngZ:{}  Hx:{}  Hy:{}  Hy:{}"
        .format(device.get("CanID"), device.get("canmode_2"), device.get("canmode_1"), device.get("AccX"),
                device.get("AccY"),
                device.get("AccZ"), device.get("AsX"), device.get("AsY"), device.get("AsZ"), device.get("AngX"),
                device.get("AngY"), device.get("AngZ"), device.get("HX"), device.get("HY"), device.get("HZ")))
    timestamp = time.time()
    """
    global last_call_time
    last_call_time = time.time()
    IMU_DATA = np.array([timestamp, DeviceModel.get("AngX"), DeviceModel.get("AngY"), DeviceModel.get("AngZ"),  \
                            DeviceModel.get("AsX"), DeviceModel.get("AsY"), DeviceModel.get("AsZ"), \
                            DeviceModel.get("AccX"), DeviceModel.get("AccY"), DeviceModel.get("AccZ"),  \
                            device.get("HX"), device.get("HY"), device.get("HZ")])
    # print(IMU_DATA)
    with open(IMU_data_file, "a") as f:
        writer = csv.writer(f)
        writer.writerow(IMU_DATA)

# shared_memory = None
# subProcStatus = None

def idCheck(DeviceModel, devID, code = 0x00):
    global subProcStatus
    logging.basicConfig(filename='Data/log.txt', level=logging.INFO, filemode='w')
    if devID == 0x50 and code == 0x00:
        subProcStatus[0] = 0x10    # 初始化成功
        logging.info(f"{datetime.datetime.now()} - {DeviceModel.deviceName} : 设备自检正常")
        print("{} : 设备自检正常".format(DeviceModel.deviceName))
    elif devID != 0x50 and code == 0x00:
        subProcStatus[0] = 0x01    # 初始化失败
        logging.info(f"{datetime.datetime.now()} - {DeviceModel.deviceName} : 设备自检失败")
        print("{} : 设备自检失败".format(DeviceModel.deviceName))
    elif code == 0x02:
        subProcStatus[0] = 0x02    # 设备丢失
        logging.info(f"{datetime.datetime.now()} - {DeviceModel.deviceName} : 设备串口 {DeviceModel.serialConfig.portName} 丢失")
        print("{} : 设备串口 {} 丢失".format(DeviceModel.deviceName,DeviceModel.serialConfig.portName))

if __name__ == "__main__":
    _fileName = sys.argv[1]
    checkProcess(_fileName)
    global shared_memory
    global subProcStatus
    if _fileName != "test" and _fileName != "i":
        # 从命令行参数中获取共享内存名称
        shared_memory_name = sys.argv[2]
        # 在子进程中打开共享内存
        shared_memory = multiprocessing.shared_memory.SharedMemory(name = shared_memory_name)
        # 将共享内存映射到一个 NumPy 数组
        subProcStatus = np.ndarray((1,), dtype=np.int32, buffer=shared_memory.buf)
        print("{} : 读取共享内存,当前预设状态为 : {}".format(sys.argv[0], subProcStatus[0]))

    if not os.path.exists(Datafolder):
        os.makedirs(Datafolder)
    with open(IMU_data_file, "a") as f:
        writer = csv.writer(f)
        writer.writerow(["Time","AngX","AngY","AngZ","AsX","AsY","AsZ","AccX","AccY","AccZ","HX","HY","HZ"])
    
    # 拿到设备模型 Get the device model
    # DeviceModel("设备名称可以自定义", "COM口号", "串口波特率", "CAN波特率(单位K)")
    device = HWT9073_CAN.DeviceModel("HWT9073-CAN IMU", "/dev/ttyUSB0", 2000000, 250, updateData, idCheck)
    # 开启设备 Turn on the device,并设置为关闭AT模式（开启透传模式）
    if not device.openDevice(True): # 打开设备失败
        idCheck(device, -1) # 反正自检失败就完了
        # time.sleep(5)  # 等待主线程响应
        print("{} : 端口 {} 打开失败, 退出!".format(device.deviceName, device.serialPort))
        # 在主程序退出时手动清理资源
        # shared_memory.close()
        sys.exit(1)
    
    """设置文件的权限和所有者"""
    uid = pwd.getpwnam('pi').pw_uid  # 获取 pi 用户的用户ID
    gid = grp.getgrnam('pi').gr_gid  # 获取 pi 用户的组ID
    # 修改目录权限，使 pi 用户可以读、写、执行
    os.chmod(os.path.dirname(Datafolder), 0o777)
    # 更改目录的所有者为 pi 用户
    os.chown(os.path.dirname(Datafolder), uid, gid)
    os.chmod(IMU_data_file, 0o666)       # 所有人可读写
    os.chown(IMU_data_file, uid, gid)    # 更改所有者为 pi 用户
    # # 设置IMU设备内部采样频率为256hz，过高会导致数据抖动较大，过低会导致设备灵敏性下降
    # device.set_bandwidth(HWT9073_CAN_BandWidth.bw_256hz)    # 上位机设置过，此处无需再次设置
    
    # # 设置IMU数据回传速率，建议100hz，或200hz，
    # 当设置为100hz时，IMU采样频率应至少设置为188hz，当设置200hz时，IMU采样频率（带宽）应设置为256hz
    # device.set_RRATE(HWT9073_CAN_RRATE.R_100hz)    # 上位机设置过，此处无需再次设置

    # 开启接收数据线程
    device.beginRecvDataThread()
    time.sleep(0.1) # 等几帧读取ID,1s大概是100帧
    if _fileName != "test" and _fileName != "i":
        while subProcStatus[0] != 0x10: # 如果读取设备ID不正常，就一直读取
            device.readReg(0x1A)
            time.sleep(0.1) # 需要时间切换读写
    device.setET() # 
    # 开启检查上次调用时间线程
    last_call_thread = threading.Thread(target=checkLastCallTime)
    last_call_thread.start()